#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Int32.h>
#include <std_msgs/String.h>
#include "opencv2/opencv.hpp"
#include <zbar.h>
#include <iostream>
#include <string>
#include <vector>
#include <chrono>
#include <thread>

using namespace std;

bool scan_flag = false;
// 初始化两个计数器
int barcode_count = 0;
int qrcode_count = 0;
float save_interval = 0.3; // 保存间隔时间（秒）
float save_distance = 150; // 保存距离阈值
std::chrono::steady_clock::time_point last_save_time = std::chrono::steady_clock::now();

//test message
// std_msgs::String test_msg;
// test_msg.data = "RGB:RED";

void scanCallback(const std_msgs::Bool::ConstPtr& msg) {
    scan_flag = msg->data;
}

float calculateDistance(cv::Point2f point1, cv::Point2f point2) {
    return sqrt(pow(point1.x - point2.x, 2) + pow(point1.y - point2.y, 2));
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "qr_code_scanner");
    ros::NodeHandle nh;

    ros::Subscriber scan_sub = nh.subscribe("scan_flag", 10, scanCallback);
    ros::Publisher result_pub = nh.advertise<std_msgs::Int32>("scan_result", 10);
    ros::Publisher color_pub = nh.advertise<std_msgs::String>("/PC_TO_MCU_TOP", 10);
    ros::Publisher send_pub = nh.advertise<std_msgs::String>("/can_send", 10);

    cv::VideoCapture cap(2, cv::CAP_V4L2);
    if (!cap.isOpened()) {
        ROS_ERROR("can't open camera");
        return -1;
    }

    cap.set(cv::CAP_PROP_FOURCC, cv::VideoWriter::fourcc('M', 'J', 'P', 'G'));

    zbar::ImageScanner scanner;
    scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1);

    ros::Rate loop_rate(30);
    while (ros::ok()) {
        cv::Mat frame;
        cap >> frame;
        if (frame.empty()) {
            ROS_ERROR("can't get frame");
            break;
        }

        scan_flag = true;
        std_msgs::Int32 result_msg;

        // 保存未加绿色框的图像副本
        cv::Mat raw_frame = frame.clone();

        if (scan_flag) {
            cv::Mat gray;
            cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY);

            zbar::Image image(frame.cols, frame.rows, "Y800", gray.data, frame.cols * frame.rows);
            int n = scanner.scan(image);

            bool found = false;
            cv::Point2f qr_center(0, 0);  // The center of the QR code
            
            // 检查是否有符号被检测到
            if (image.symbol_begin() != image.symbol_end()) {
                for (auto symbol = image.symbol_begin(); symbol != image.symbol_end(); ++symbol) {
                    try {
                        int data = std::stoi(symbol->get_data());
                        std::cout << "Decoded Data: " << symbol->get_data() << std::endl;
                        std::cout << "解码类型: " << symbol->get_type_name() << std::endl;

                        if (data >= 1 && data <= 24) {
                            result_msg.data = data;
                            found = true;
                        }
                        
                        // 计算QR码的中心位置
                        std::vector<cv::Point> points;
                        for (int i = 0; i < symbol->get_location_size(); i++) {
                            points.push_back(cv::Point(symbol->get_location_x(i), symbol->get_location_y(i)));
                        }
                        cv::polylines(frame, points, true, cv::Scalar(0, 255, 0), 2);

                        // 计算QR码中心
                        if (!points.empty()) {
                            cv::Moments m = cv::moments(points);
                            if (m.m00 != 0) {
                                qr_center = cv::Point2f(m.m10 / m.m00, m.m01 / m.m00);
                            }
                        }
                    } catch (const std::invalid_argument& e) {
                        std::cout << "无法将数据转换为整数: " << symbol->get_data() << std::endl;
                    } catch (const std::out_of_range& e) {
                        std::cout << "数据超出整数范围: " << symbol->get_data() << std::endl;
                    }
                }

                // 获取图像的中心
                cv::Point2f image_center(frame.cols / 2, frame.rows / 2);

                // 计算QR码中心和图像中心的距离
                float distance = calculateDistance(qr_center, image_center);
                std::cout << "Distance to center: " << distance << std::endl;

                // 判断是否保存图片
                auto now = std::chrono::steady_clock::now();
                std::chrono::duration<double> elapsed = now - last_save_time;

                // 安全获取符号类型
                std::string type = "";
                auto symbol = image.symbol_begin();
                if (symbol != image.symbol_end()) {
                    type = symbol->get_type_name();
                }

                std::string filename;

                // 条形码不检测距离
                if ((type.find("EAN") != std::string::npos || type.find("CODE") != std::string::npos) && barcode_count < 3) {
                    if (elapsed.count() >= save_interval) {
                        barcode_count++;
                        filename = "/home/firefly/temp/" + std::to_string(barcode_count) + ".jpg";
                        cv::imwrite(filename, raw_frame);
                        std::cout << "Barcode saved: " << filename << std::endl;
                        last_save_time = now;
                    }
                    // 立即发送 RGB:RED
                    std_msgs::String color_msg;
                    color_msg.data = "RGB:RED";
                    color_pub.publish(color_msg);
                }
                // 二维码需要检测距离
                else if (type.find("QR") != std::string::npos && barcode_count >= 3 && qrcode_count < 3 && distance < save_distance) {
                    if (elapsed.count() >= save_interval) {
                        qrcode_count++;
                        filename = "/home/firefly/temp/" + std::to_string(3 + qrcode_count) + ".jpg";
                        cv::imwrite(filename, raw_frame);
                        std::cout << "QR code saved: " << filename << std::endl;
                        last_save_time = now;
                    }
                }
            } else {
                // 没有检测到符号
                result_msg.data = -1;
            }

            if (!found) {
                result_msg.data = -1;
            }
            
            // 全部保存完毕，退出程序
            if (barcode_count == 3 && qrcode_count == 3) {
                std::cout << "All images saved. Exiting." << std::endl;
                std_msgs::String send_msg;
                send_msg.data = "yes";
                send_pub.publish(send_msg);
                return 0;
            }
            
        } else {
            result_msg.data = -1;
        }

        result_pub.publish(result_msg);
        std::cout << "Scan Result: " << result_msg.data << std::endl;
        cv::imshow("Camera Feed", frame);

        if (cv::waitKey(1) == 27) {
            break;
        }

        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}

